//////////////////////////////////////////////
//	File:		Math.h
//	Aurthor:	Sebastian Lewicki
//	Purpose:	Basic math library
/////////////////////////////////////////////

#ifndef _CMATH_H_
#define _CMATH_H_

#include <algorithm>
#include <cmath>

#define PI 3.14159f

#pragma region Functions

// Prototype
static float DegToRad(float _fDegree);
static float RadToDeg(float _fRadian);

//Functions
float DegToRad(float _fDegree)
{
	return _fDegree * PI / 180.0f;
}

float RadToDeg(float _fRadian)
{
	return _fRadian * 180.0f / PI;
}

#pragma endregion

#pragma region Vectors

// Vector definitions
#define VEC3_BINARY_COMPONENT_OPERATOR(_oper_) \
	vec3<element_type> operator _oper_ (const_reference v) const\
   { \
   vec3<element_type> a; \
   a.x = x _oper_ v.x; \
   a.y = y _oper_ v.y; \
   a.z = z _oper_ v.z; \
   return a; \
   }

#define VEC3_BINARY_BROADCAST_OPERATOR(_oper_) \
	template <typename FT> \
	vec3<element_type> operator _oper_ (FT fValue) const \
   { \
   vec3<element_type> a; \
   a.x = x _oper_ fValue; \
   a.y = y _oper_ fValue; \
   a.z = z _oper_ fValue; \
   return a; \
   }

#define VEC3_COMPONENT_OPERATOR(_oper_) \
	reference operator _oper_ (const_reference v) \
   { \
   if (static_cast<void *>(this) != static_cast<const void *>(&v)) \
	  { \
	  x _oper_ v.x; \
	  y _oper_ v.y; \
	  z _oper_ v.z; \
	  } \
	  return *this; \
   }

#define VEC3_BROADCAST_OPERATOR(_oper_) \
	template <typename FT> \
	reference operator _oper_ (FT fValue) \
   { \
   x _oper_ fValue; \
   y _oper_ fValue; \
   z _oper_ fValue; \
   return *this; \
   }

template <typename T>
struct vec3
{
	typedef vec3 &reference;
	typedef vec3 const &const_reference;
	typedef T element_type;

	union
	{
		element_type v[3];
		struct
		{
			element_type x;
			element_type y;
			element_type z;
		};
	};

	vec3(void) { }
	~vec3(void) { }
	vec3(element_type vx, element_type vy, element_type vz) : x(vx), y(vy), z(vz) { }

	template <typename VT>
	vec3(const vec3<VT> &v) : x(v.x), y(v.y), z(v.z) { }

	VEC3_BINARY_COMPONENT_OPERATOR(+);
	VEC3_BINARY_COMPONENT_OPERATOR(-);
	VEC3_BINARY_COMPONENT_OPERATOR(*);
	VEC3_BINARY_COMPONENT_OPERATOR(/);

	VEC3_BINARY_BROADCAST_OPERATOR(+);
	VEC3_BINARY_BROADCAST_OPERATOR(-);
	VEC3_BINARY_BROADCAST_OPERATOR(*);
	VEC3_BINARY_BROADCAST_OPERATOR(/);

	VEC3_COMPONENT_OPERATOR(=);
	VEC3_COMPONENT_OPERATOR(+=);
	VEC3_COMPONENT_OPERATOR(-=);
	VEC3_COMPONENT_OPERATOR(*=);
	VEC3_COMPONENT_OPERATOR(/=);

	VEC3_BROADCAST_OPERATOR(=);
	VEC3_BROADCAST_OPERATOR(+=);
	VEC3_BROADCAST_OPERATOR(-=);
	VEC3_BROADCAST_OPERATOR(*=);
	VEC3_BROADCAST_OPERATOR(/=);

	reference make_zero(void)
	{
		x = y = z = 0;
		return *this;
	}

	reference normalize(void)
	{
		element_type rsqrt = 1 / magnitude();
		x *= rsqrt;
		y *= rsqrt;
		z *= rsqrt;
		return *this;
	}

	reference negate(void)
	{
		x = -x;
		y = -y;
		z = -z;
		return *this;
	}

	element_type magnitude(void)
	{
		element_type fMagnitude;
		fMagnitude = (T)sqrt(x*x + y*y + z*z);
		return fMagnitude;
	}

	element_type lenghtsq()
	{
		return (T)(x*x + y*y + z*z);
	}

	reference cross(reference Left, reference Right)
	{
		x = (Left.y * Right.z) - (Right.y * Left.z);
		y = -(Left.z * Right.x) + (Left.x * Right.z);
		z = (Left.x * Right.y) - (Right.x * Left.y);
		return *this;
	}

	element_type dot(reference Left, reference Right)
	{
		return (Left.x * Right.x) + (Left.y * Right.y) + (Left.z * Right.z);
	}

	element_type dot(reference Right)
	{
		return (x * Right.x) + (y * Right.y) + (z * Right.z);
	}

	element_type *array(void) { return this->v; }
	element_type const *array(void) const { return this->v; }
};


typedef vec3<float> vec3f;
typedef vec3<double> vec3d;
typedef vec3f float3;
typedef vec3d double3;

#pragma endregion

#pragma region Matrix // 3x3

// 3x3 Matrix definitions
template <typename T>
struct matrix3
{
	typedef T element_type;
	typedef matrix3 &reference;
	typedef matrix3 const &const_reference;
	typedef matrix3 blas_class;

	union
	{
		element_type m[3][3];
		element_type ma[9];
		struct
		{
			element_type xx, xy, xz;
			element_type yx, yy, yz;
			element_type zx, zy, zz;
		};
		struct
		{
			vec3<element_type> axis_x;
			vec3<element_type> axis_y;
			vec3<element_type> axis_z;
		};
	};

	matrix3(void) { }
	explicit matrix3(element_type n);

	template < typename VT >
	matrix3(const vec3<VT> &v0,
		const vec3<VT> &v1,
		const vec3<VT> &v2);

	matrix3(element_type m00, element_type m01, element_type m02,
		element_type m10, element_type m11, element_type m12,
		element_type m20, element_type m21, element_type m22);

	// TODO: I'm only pretty sure this should be explicit
	// I think this counts as a one argument constructor, not as a copy constructor.
	// The question is, does this replace the copy constructor when T == MT ?
	template <typename MT>
	explicit matrix3(const matrix3<MT> &m);

	template < typename VT >
	matrix3(const vec3<VT> &diag, const vec3<VT> &sym);

	// TODO: is this any faster than the built in one?
	// TODO: does this work with UDTs that are POD and if so,
	// is that desired and defined behavior?
	template <typename MT>
	reference operator=(const matrix3<MT> &m);

	inline T operator[](size_t index)
	{
		return ma[index];
	}

	inline const T operator[](size_t index) const
	{
		return ma[index];
	}

	template <typename MT>
	matrix3<T> operator*(const matrix3<MT> &rhs);

	template <typename MT>
	reference operator*=(const matrix3<MT> &rhs);

	reference make_zero(void);
	reference make_identity(void);

	// "this" BECOMES the specified rotation matrix
	// make_rotation_ does NOT rotate "this", it constructs
	// a rotation matrix of 'radians' on the specified axis.
	reference make_rotation(element_type radians, vec3<element_type> _Vector);

	reference negate(void);
};

template <typename T>
template <typename VT>
matrix3<T>::matrix3(const vec3<VT> &v0, const vec3<VT> &v1, const vec3<VT> &v2)
:	xx(v0.x), xy(v0.y), xz(v0.z),
yx(v1.x), yy(v1.y), yz(v1.z),
zx(v2.x), zy(v2.y), zz(v2.z)
{
}

template <typename T>
matrix3<T>::matrix3(element_type n)
:	xx(n), xy(n), xz(n),
yx(n), yy(n), yz(n),
zx(n), zy(n), zz(n)
{
}

template <typename T>
matrix3<T>::matrix3(element_type m00, element_type m01, element_type m02,
					element_type m10, element_type m11, element_type m12,
					element_type m20, element_type m21, element_type m22)
					:	xx(m00), xy(m01), xz(m02),
					yx(m10), yy(m11), yz(m12),
					zx(m20), zy(m21), zz(m22)
{
}

template <typename T>
template <typename MT>
matrix3<T>::matrix3(const matrix3<MT> &m)
:	xx(m.xx), xy(m.xy), xz(m.xz),
yx(m.yx), yy(m.yy), yz(m.yz),
zx(m.zx), zy(m.zy), zz(m.zz)
{
}

template <typename T>
template <typename VT>
matrix3<T>::matrix3(const vec3<VT> &diag, const vec3<VT> &sym)
{
	make_symmetric(this, diag, sym);
}

template <typename T>
template <typename MT>
typename matrix3<T>::reference matrix3<T>::operator=(const matrix3<MT> &m)
{
	if (static_cast<void *>(this) != static_cast<const void *>(&m))
	{
		xx = m.xx;	xy = m.xy;	xz = m.xz;
		yx = m.yx;	yy = m.yy;	yz = m.yz;
		zx = m.zx;	zy = m.zy;	zz = m.zz;
	}
	return *this;
}

template <typename T>
typename matrix3<T>::reference matrix3<T>::make_zero(void)
{
	xx = 0;	xy = 0;	xz = 0;
	yx = 0;	yy = 0;	yz = 0;
	zx = 0;	zy = 0;	zz = 0;
	return *this;
}

template <typename T>
typename matrix3<T>::reference matrix3<T>::make_identity(void)
{
	xx = 1;	xy = 0;	xz = 0;
	yx = 0;	yy = 1;	yz = 0;
	zx = 0;	zy = 0;	zz = 1;
	return *this;
}

template <typename T>
typename matrix3<T>::reference matrix3<T>::negate(void)
{
	*this *= -1;
	return *this;
}

template <typename T>
typename matrix3<T>::reference matrix3<T>::make_rotation(element_type radians, vec3<element_type> _Vector)
{
	float fSin, fCos;
	(*this).make_identity();

	fSin = sinf(radians);
	fCos = cosf(radians);

	xx = ((( 1.0f - fCos ) * _Vector.x * _Vector.x ) + ( fCos ));
	xy = ((( 1.0f - fCos ) * _Vector.x * _Vector.y ) - ( fSin * _Vector.z ));
	xz = ((( 1.0f - fCos ) * _Vector.x * _Vector.z ) + ( fSin * _Vector.y ));
	yx = ((( 1.0f - fCos ) * _Vector.y * _Vector.x ) + ( fSin * _Vector.z ));
	yy = ((( 1.0f - fCos ) * _Vector.y * _Vector.y ) + ( fCos ));
	yz = ((( 1.0f - fCos ) * _Vector.y * _Vector.z ) - ( fSin * _Vector.x ));
	zx = ((( 1.0f - fCos ) * _Vector.z * _Vector.x ) - ( fSin * _Vector.y ));
	zy = ((( 1.0f - fCos ) * _Vector.z * _Vector.y ) + ( fSin * _Vector.x ));
	zz = ((( 1.0f - fCos ) * _Vector.z * _Vector.z ) + ( fCos ));

	return *this;
}

template <typename T>
template <typename MT>
typename matrix3<T> matrix3<T>::operator*(const matrix3<MT> &rhs)
{
	matrix3<T> temp(xx * rhs.xx + yx * rhs.xy + zx * rhs.xz,
		xy * rhs.xx + yy * rhs.xy + zy * rhs.xz,
		xz * rhs.xx + yz * rhs.xy + zz * rhs.xz,

		xx * rhs.yx + yx * rhs.yy + zx * rhs.yz,
		xy * rhs.yx + yy * rhs.yy + zy * rhs.yz,
		xz * rhs.yx + yz * rhs.yy + zz * rhs.yz,

		xx * rhs.zx + yx * rhs.zy + zx * rhs.zz,
		xy * rhs.zx + yy * rhs.zy + zy * rhs.zz,
		xz * rhs.zx + yz * rhs.zy + zz * rhs.zz);
	return(temp);
}

template <typename T>
template <typename MT>
typename matrix3<T>::reference matrix3<T>::operator*=(const matrix3<MT> &rhs)
{
	*this = *this * rhs;
	return *this;
}

typedef matrix3<float> matrix3f;
typedef matrix3<double> matrix3d;
typedef matrix3f float3x3;
typedef matrix3d double3x3;

#pragma endregion

#pragma region Matrix // 4x4

// 4x4 Matrix definitions
#pragma intrinsic(sin, cos)

template <typename T>
struct matrix4
{
	typedef T element_type;
	typedef matrix4 &reference;
	typedef matrix4 const &const_reference;

	union
	{
		element_type m[4][4];
		element_type ma[16];
		struct
		{
			element_type xx, xy, xz, xw;
			element_type yx, yy, yz, yw;
			element_type zx, zy, zz, zw;
			element_type wx, wy, wz, ww;
		};
		struct
		{
			vec3<element_type> axis_x;	element_type pad_xw;
			vec3<element_type> axis_y;	element_type pad_yw;
			vec3<element_type> axis_z;	element_type pad_zw;
			union
			{
				struct
				{
					vec3<element_type> axis_w;	element_type pad_ww;
				};
				struct
				{
					vec3<element_type> axis_pos;
				};
			};
		};
	};

	// Constructors
	matrix4(void) { }
	explicit matrix4(element_type n);
	matrix4(element_type m00, element_type m01, element_type m02, element_type m03,
		element_type m10, element_type m11, element_type m12, element_type m13,
		element_type m20, element_type m21, element_type m22, element_type m23,
		element_type m30, element_type m31, element_type m32, element_type m33);

	inline T operator[](size_t index)
	{
		return ma[index];
	}

	inline const T operator[](size_t index) const
	{
		return ma[index];
	}

	template <typename MT>
	reference operator*(const matrix4<MT> &rhs);

	template <typename MT>
	reference operator*=(const matrix4<MT> &rhs);

	vec3<element_type> scale(void) const;

	reference make_zero(void);
	reference make_identity(void);

	// "this" BECOMES the specified rotation matrix
	// make_rotation_ does NOT rotate "this", it constructs
	// a rotation matrix of 'radians' on the specified axis.
	reference make_rotation(element_type radians, vec3<element_type> _Vector);

	// "this" BECOMES the specified scale matrix
	reference make_scale(element_type x, element_type y, element_type z);

	template <typename VT>
	reference make_scale(const vec3<VT> &v);

	reference invert(void);
	reference transpose(void);
	reference negate(void);

	// Transposes the 3x3 and inverts position.  (Must be orthonormal)
	//	void fast_invert(void);

	// nVidia wrote this.  I do not know if this is working.
	//	bool calcProjectionMatrixDeg(float fFOV_degrees, float fAspect, float Znear, float Zfar);

	// normalize each 3x1 vector of the 3x3 subset
	reference normalize3x3vectors(void);
};

template <typename T>
matrix4<T>::matrix4(element_type n)
:	xx(n), xy(n), xz(n), xw(n),
yx(n), yy(n), yz(n), yw(n),
zx(n), zy(n), zz(n), zw(n),
wx(n), wy(n), wz(n), ww(n)
{
}

template <typename T>
matrix4<T>::matrix4(element_type m00, element_type m01, element_type m02, element_type m03,
					element_type m10, element_type m11, element_type m12, element_type m13,
					element_type m20, element_type m21, element_type m22, element_type m23,
					element_type m30, element_type m31, element_type m32, element_type m33)
					:	xx(m00), xy(m01), xz(m02), xw(m03),
					yx(m10), yy(m11), yz(m12), yw(m13),
					zx(m20), zy(m21), zz(m22), zw(m23),
					wx(m30), wy(m31), wz(m32), ww(m33)
{
}

template <typename T>
vec3<T> matrix4<T>::scale(void) const
{
	return vec3<T>(xx, yy, zz);
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::make_zero(void)
{
	// TODO: would memset be faster here?
	xx = 0;	xy = 0;	xz = 0;	xw = 0;
	yx = 0;	yy = 0;	yz = 0;	yw = 0;
	zx = 0;	zy = 0;	zz = 0;	zw = 0;
	wx = 0;	wy = 0;	ww = 0;	ww = 0;
	return *this;
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::make_identity(void)
{
	xx = 1;	xy = 0;	xz = 0;	xw = 0;
	yx = 0;	yy = 1;	yz = 0;	yw = 0;
	zx = 0;	zy = 0;	zz = 1;	zw = 0;
	wx = 0;	wy = 0;	wz = 0;	ww = 1;
	return *this;
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::negate(void)
{
	*this *= -1;
	return *this;
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::make_rotation(element_type radians, vec3<element_type> _Vector)
{
	element_type sin_angle, cos_angle;
	sin_angle = std::sin(radians);
	cos_angle = std::cos(radians);

	xx = ((( 1.0f - cos_angle ) * _Vector.x * _Vector.x ) + ( cos_angle ));
	xy = ((( 1.0f - cos_angle ) * _Vector.x * _Vector.y ) - ( sin_angle * _Vector.z ));
	xz = ((( 1.0f - cos_angle ) * _Vector.x * _Vector.z ) + ( sin_angle * _Vector.y ));
	xw = 0.0;
	yx = ((( 1.0f - cos_angle ) * _Vector.y * _Vector.x ) + ( sin_angle * _Vector.z ));
	yy = ((( 1.0f - cos_angle ) * _Vector.y * _Vector.y ) + ( cos_angle ));
	yz = ((( 1.0f - cos_angle ) * _Vector.y * _Vector.z ) - ( sin_angle * _Vector.x ));
	yw = 0.0;
	zx = ((( 1.0f - cos_angle ) * _Vector.z * _Vector.x ) - ( sin_angle * _Vector.y ));
	zy = ((( 1.0f - cos_angle ) * _Vector.z * _Vector.y ) + ( sin_angle * _Vector.x ));
	zz = ((( 1.0f - cos_angle ) * _Vector.z * _Vector.z ) + ( cos_angle ));
	zw = 0.0;
	wx = 0.0;
	wy = 0.0;
	wz = 0.0;
	ww = 1.0;

	return *this;
}

template <typename T>
typename matrix4<T>::reference
matrix4<T>::make_scale(element_type x, element_type y, element_type z)
{
	xx = x;	xy = 0;	xz = 0;	xw = 0;
	yx = 0;	yy = y;	yz = 0;	yw = 0;
	zx = 0;	zy = 0;	zz = z;	zw = 0;
	wx = 0;	wy = 0;	wz = 0;	ww = 1;
	return *this;
}

template <typename T>
template <typename VT>
typename matrix4<T>::reference matrix4<T>::make_scale(const vec3<VT> &v)
{
	xx = v.x;	xy = 0;		xz = 0;		xw = 0;
	yx = 0;		yy = v.y;	yz = 0;		yw = 0;
	zx = 0;		zy = 0;		zz = v.z;	zw = 0;
	wx = 0;		wy = 0;		wz = 0;		ww = 1;
	return *this;
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::transpose(void)
{
	std::swap(xy, yx);
	std::swap(xz, zx);
	std::swap(xw, wx);	
	std::swap(yz, zy);
	std::swap(yw, wy);
	std::swap(zw, wz);
	return *this;
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::invert(void)
{
	*this = matrix4<T>(xx, yx, zx, 0,
		xy, yy, zy, 0,
		xz, yz, zz, 0,
		-(wx * xx + wy * xy + wz * xz),
		-(wx * yx + wy * yy + wz * yz),
		-(wx * zx + wy * zy + wz * zz), 1);
	return *this;
}

template <typename T>
typename matrix4<T>::reference matrix4<T>::normalize3x3vectors(void)
{
	axis_x().normalize();
	axis_y().normalize();
	axis_z().normalize();
}

template <typename T>
template <typename MT>
typename matrix4<T>::reference matrix4<T>::operator*(const matrix4<MT> &rhs)
{
	matrix4<T> temp(xx * rhs.xx + yx * rhs.xy + zx * rhs.xz + wx * rhs.xw,
		xy * rhs.xx + yy * rhs.xy + zy * rhs.xz + wy * rhs.xw,
		xz * rhs.xx + yz * rhs.xy + zz * rhs.xz + wz * rhs.xw,
		xw * rhs.xx + yw * rhs.xy + zw * rhs.xz + ww * rhs.xw,

		xx * rhs.yx + yx * rhs.yy + zx * rhs.yz + wx * rhs.yw,
		xy * rhs.yx + yy * rhs.yy + zy * rhs.yz + wy * rhs.yw,
		xz * rhs.yx + yz * rhs.yy + zz * rhs.yz + wz * rhs.yw,
		xw * rhs.yx + yw * rhs.yy + zw * rhs.yz + ww * rhs.yw,

		xx * rhs.zx + yx * rhs.zy + zx * rhs.zz + wx * rhs.zw,
		xy * rhs.zx + yy * rhs.zy + zy * rhs.zz + wy * rhs.zw,
		xz * rhs.zx + yz * rhs.zy + zz * rhs.zz + wz * rhs.zw,
		xw * rhs.zx + yw * rhs.zy + zw * rhs.zz + ww * rhs.zw,

		xx * rhs.wx + yx * rhs.wy + zx * rhs.wz + wx * rhs.ww,
		xy * rhs.wx + yy * rhs.wy + zy * rhs.wz + wy * rhs.ww,
		xz * rhs.wx + yz * rhs.wy + zz * rhs.wz + wz * rhs.ww,
		xw * rhs.wx + yw * rhs.wy + zw * rhs.wz + ww * rhs.ww);
	*this = temp;
	return *this;
}

template <typename T>
template <typename MT>
typename matrix4<T>::reference matrix4<T>::operator*=(const matrix4<MT> &rhs)
{
	*this = *this * rhs;
	return *this;
}

#pragma function(sin, cos)

typedef matrix4<float> matrix4f;
typedef matrix4<double> matrix4d;
typedef matrix4f float4x4;
typedef matrix4d double4x4;

#pragma endregion

#endif